﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace QuMianChaZhi.SurfaceInterpolation
{
    public struct Mat
    {
        public int row;
        public int col;
        public double num;
    }
    public class InterpolationMethodFour : InterpolationMethod
    {
        private List<Point> mInitPoint;

        public double[,] CalculateInterpolation(List<Point> initPoints)
        {
            mInitPoint = initPoints;

            double[,] ret = new double[101, 101];

            double[,] h = CalculateDistance();
            h = DistanceSort(h);
            List<List<double>> Num_h = CountNumberDistance(h);
            List<List<double>> yh_E = PredictY_h(h, Num_h);
            double[] P = CoeffPSO(yh_E, Num_h);

            for (int x = -50; x < 50; x++)
            {
                for (int y = -50; y < 50; y++)
                {
                    double z = CalculateValue(x, y, P);

                    ret[x + 50, y + 50] = z;
                }
            }

            return ret;
        }

        private double[,] CalculateDistance()
        {
            int N = (mInitPoint.Count * (mInitPoint.Count - 1)) / 2;
            double[,] h = new double[N, 3];
            int temp = 0;
            for (int i = 0; i < mInitPoint.Count - 1; i++)
            {
                for (int j = i + 1; j < mInitPoint.Count; j++)
                {
                    h[temp, 0] = i;
                    h[temp, 1] = j;
                    h[temp, 2] = Math.Sqrt(Math.Pow((mInitPoint[i].x - mInitPoint[j].x), 2) + Math.Pow((mInitPoint[i].y - mInitPoint[j].y), 2));
                    temp++;
                }
            }
            return h;
        }

        private double[,] DistanceSort(double[,] h)
        {
            int N = (mInitPoint.Count * (mInitPoint.Count - 1)) / 2;
            double temp = 0;
            for (int i = 0; i < N - 1; i++)
            {
                for (int j = i + 1; j < N; j++)
                {
                    if (h[j, 2] < h[i, 2])
                    {
                        temp = h[j, 0];
                        h[j, 0] = h[i, 0];
                        h[i, 0] = temp;
                        temp = h[j, 1];
                        h[j, 1] = h[i, 1];
                        h[i, 1] = temp;
                        temp = h[j, 2];
                        h[j, 2] = h[i, 2];
                        h[i, 2] = temp;
                    }
                }
            }
            return h;
        }


        private List<List<double>> CountNumberDistance(double[,] h)
        {
            List<List<double>> Num_h = new List<List<double>>();
            List<double> Num_h_0 = new List<double>();
            List<double> Num_h_1 = new List<double>();
            int N = (mInitPoint.Count * (mInitPoint.Count - 1)) / 2;
            int temp = 0;
            for (int i = 0; i < N - 1; i++)
            {
                if (h[i, 2] == h[i + 1, 2])
                {
                    temp++;
                }
                else
                {
                    Num_h_0.Add(h[i, 2]);
                    Num_h_1.Add(temp + 1);
                    temp = 0;
                }
            }
            Num_h.Add(Num_h_0);
            Num_h.Add(Num_h_1);
            return Num_h;
        }

        private List<List<double>> PredictY_h(double[,] h, List<List<double>> Num_h)
        {
            int n = Num_h[0].Count;
            int temp = 0;
            List<List<double>> yh_E = new List<List<double>>();
            List<double> yh_E_0 = new List<double>();
            List<double> yh_E_1 = new List<double>();
            double[,] temp_yh_E = new double[n, 3];
            for (int i = 0; i < n; i++)
            {
                double temp_yh = 0;
                for (int j = temp; j < (temp + (int)Num_h[1][i]); j++)
                {
                    temp_yh = temp_yh + Math.Pow((mInitPoint[(int)h[j, 0]].z - mInitPoint[(int)h[j, 1]].z), 2);
                }
                temp_yh /= (2 * Num_h[1][i]);
                temp_yh_E[i, 0] = Num_h[0][i];
                temp_yh_E[i, 1] = Num_h[1][i];
                temp_yh_E[i, 2] = temp_yh;
                temp += (int)Num_h[1][i];
            }
            for (int i = 0; i < n; i++)
            {
                if (temp_yh_E[i, 1] >= 3)
                {
                    yh_E_0.Add(temp_yh_E[i, 0]);
                    yh_E_1.Add(temp_yh_E[i, 2]);
                }
            }
            yh_E.Add(yh_E_0);
            yh_E.Add(yh_E_1);
            return yh_E;
        }

        private double RAND()
        {
            Random r = new Random();
            int i = r.Next(0, 10000);
            double I = ((double)i) / 10000.0;
            return I;
        }

        private int ROUND(double x)
        {
            int temp = (int)x;
            double temp1 = temp;
            if ((temp1 + 1 - x) > 0.5)
                return temp;
            else
                return (temp + 1);
        }

        private double NORMRND()
        {
            Random rand = new Random();
            double u1, u2, v1 = 0, v2 = 0, s = 0, z1 = 0, z2 = 0;
            while (s > 1 || s == 0)
            {
                u1 = rand.NextDouble();
                u2 = rand.NextDouble();
                v1 = 2 * u1 - 1;
                v2 = 2 * u2 - 1;
                s = v1 * v1 + v2 * v2;
            }
            z1 = Math.Sqrt(-2 * Math.Log(s) / s) * v1;
            z2 = Math.Sqrt(-2 * Math.Log(s) / s) * v2;
            if (RAND() >= 0.5)
                return z1;
            else
                return z2;
        }

        private double[] CoeffPSO(List<List<double>> yh_E, List<List<double>> Num_h)
        {
            int n = yh_E[0].Count;
            double[] P = new double[3];
            for (int d = 0; d < 30; d++)
            {
                //double C0_max = 0.0000005, C_max = 0.2, a_max = 700;
                //double C0_min = 0, C_min = 0, a_min = 600;

                double C0_max = 0.1, C_max = 0.1, a_max = 650;
                double C0_min = 0, C_min = 0, a_min = 500;

                double temp_C0 = C0_min + RAND() * C0_max, temp_C = C_min + RAND() * C_max, temp_a = a_min + RAND() * a_max;
                double P_C0 = temp_C0, P_C = temp_C, P_a = temp_a;
                double last = 0, now = 0;
                double wmax = 3, wmin = 1;

                //double vC0_max = C0_max / 20, vC_max = 0.005, va_max = 3;

                double vC0_max = 0.005, vC_max = 0.005, va_max = 3;

                double vC0_l = RAND() * vC0_max, vC0_n = 0;
                double vC_l = RAND() * vC_max, vC_n = 0;
                double va_l = RAND() * va_max, va_n = 0;
                double e = 100000;
                for (int i = 0; i < n; i++)
                {
                    //double temp = -Math.Pow((Num_h[0][i] / temp_a), 2);
                    //double temp1 = temp_C0 + temp_C * (1 - Math.Exp(temp));
                    //last += Math.Pow((temp1 - yh_E[1][i]), 2);

                    double temp = 3.0 / 2.0 * Num_h[0][i] / temp_a - Math.Pow(Num_h[0][i], 3) / (2 * Math.Pow(temp_a, 3));
                    double temp1 = temp_C0 + temp_C * temp;
                    last += Math.Pow((temp1 - yh_E[1][i]), 2);
                }
                List<double> TEMP_0 = new List<double>();
                List<double> TEMP_1 = new List<double>();
                List<double> TEMP_2 = new List<double>();
                for (int i = 0; i < 500; i++)
                {
                    TEMP_0.Add(temp_C0); TEMP_1.Add(temp_C); TEMP_2.Add(temp_a);
                    double w = wmax - (wmax - wmin) * (i / 50);
                    vC0_n = w * vC0_l + 2 * RAND() * (P_C0 - temp_C0) + 2 * RAND() * Math.Sqrt(Math.Pow((P_C0 - temp_C0), 2) + Math.Pow((P_C - temp_C), 2) + Math.Pow((P_a - temp_a), 2));
                    vC0_n = (vC0_n > vC0_max) ? vC0_max : ((vC0_n < -vC0_max) ? -vC0_max : vC0_n);
                    temp_C0 += vC0_n;
                    vC_n = w * vC_l + 2 * RAND() * (P_C - temp_C) + 2 * RAND() * Math.Sqrt(Math.Pow((P_C0 - temp_C0), 2) + Math.Pow((P_C - temp_C), 2) + Math.Pow((P_a - temp_a), 2));
                    vC_n = (vC_n > vC_max) ? vC_max : ((vC_n < -vC_max) ? -vC_max : vC_n);
                    temp_C += vC_n;
                    va_n = w * va_l + 2 * RAND() * (P_a - temp_a) + 2 * RAND() * Math.Sqrt(Math.Pow((P_C0 - temp_C0), 2) + Math.Pow((P_C - temp_C), 2) + Math.Pow((P_a - temp_a), 2));
                    va_n = (va_n > va_max) ? va_max : ((va_n < -va_max) ? -va_max : va_n);
                    temp_a += va_n;

                    temp_C0 = 0;

                    for (int j = 0; j < n; j++)
                    {
                        //double temp = -Math.Pow((Num_h[0][j] / temp_a), 2);
                        //double temp1 = temp_C0 + temp_C * (1 - Math.Exp(temp));
                        //now += Math.Pow((temp1 - yh_E[1][j]), 2);

                        double temp = 3.0 / 2.0 * Num_h[0][j] / temp_a - Math.Pow(Num_h[0][j], 3) / (2 * Math.Pow(temp_a, 3));
                        double temp1 = temp_C0 + temp_C * temp;
                        now += Math.Pow((temp1 - yh_E[1][j]), 2);
                    }
                    if ((i % 10) == 0)
                    {
                        int[] r = new int[(int)(i * 0.2)];
                        for (int temp_i = 0; temp_i < (int)(i * 0.2); temp_i++)
                        {
                            r[temp_i] = ROUND(1 + RAND() * (i - 1));
                        }
                        for (int j = 0; j < (int)(i * 0.2); j++)
                        {
                            double[] t_TEMP = new double[3];
                            double Radian = (RAND() - 1 / 2) * Math.PI;
                            double temp = 0.1 * Math.Tan(Radian);
                            t_TEMP[0] = TEMP_0[r[j]] + temp;
                            t_TEMP[1] = TEMP_1[r[j]] + temp;
                            t_TEMP[2] = TEMP_2[r[j]] + temp;
                            if ((t_TEMP[0] > C0_max) || (t_TEMP[0] < C0_min) || (t_TEMP[1] > C_max) || (t_TEMP[1] < C_min) || (t_TEMP[2] > a_max) || (t_TEMP[2] < a_max))
                            {
                                temp = 0.1 * NORMRND();
                                t_TEMP[0] = TEMP_0[r[j]] + temp;
                                t_TEMP[1] = TEMP_1[r[j]] + temp;
                                t_TEMP[2] = TEMP_2[r[j]] + temp;
                                if (t_TEMP[0] > C0_max)
                                    t_TEMP[0] = C0_max;
                                else if (t_TEMP[0] < C0_min)
                                    t_TEMP[0] = C0_min;
                                if (t_TEMP[1] > C_max)
                                    t_TEMP[1] = C_max;
                                else if (t_TEMP[1] < C_min)
                                    t_TEMP[1] = C_min;
                                if (t_TEMP[2] > a_max)
                                    t_TEMP[2] = a_max;
                                else if (t_TEMP[2] < a_min)
                                    t_TEMP[2] = a_min;
                            }
                            else
                            {
                                TEMP_0[r[j]] = t_TEMP[0];
                                TEMP_1[r[j]] = t_TEMP[1];
                                TEMP_2[r[j]] = t_TEMP[2];
                            }
                            double t = 0;
                            for (int J = 0; J < n; J++)
                            {
                                temp = -Math.Pow((Num_h[0][J] / TEMP_2[r[j]]), 2);
                                double temp1 = TEMP_0[r[j]] + TEMP_1[r[j]] * (1 - Math.Exp(temp));
                                t += Math.Pow((temp1 - yh_E[1][J]), 2);
                            }
                            if (last > t)
                            {
                                last = t;
                                P_C0 = TEMP_0[r[j]];
                                P_C = TEMP_1[r[j]];
                                P_a = TEMP_2[r[j]];
                            }
                        }
                    }
                    if (last > now)
                    {
                        P_C0 = temp_C0;
                        P_C = temp_C;
                        P_a = temp_a;
                        last = now;
                    }
                    vC0_l = vC0_n;
                    vC_l = vC_n;
                    va_l = va_n;
                    now = 0;
                }
                if (last < e)
                {
                    e = last;
                    P[0] = P_C0;
                    P[1] = P_C;
                    P[2] = P_a;
                }
            }
            return P;
        }

        private Mat MaxAbs(Mat[,] A, int k)
        {
            Mat Max = A[k, k];
            int n = A.GetLength(0);
            for (int i = k + 1; i < n; i++)
            {
                double temp1 = Math.Abs(Max.num);
                double temp2 = Math.Abs(A[i, k].num);
                Max = temp1 > temp2 ? Max : A[i, k];
            }
            return Max;
        }

        private Mat[,] exchange(int k, Mat Max, Mat[,] A)
        {
            double temp = 0;
            int n = A.GetLength(1);
            if (Max.row == k)
                return A;
            for (int i = 0; i < n; i++)
            {
                temp = A[Max.row, i].num;
                A[Max.row, i].num = A[k, i].num;
                A[k, i].num = temp;
            }
            return A;
        }

        private Mat[,] Eli(Mat[,] A)
        {
            Mat Max;
            for (int i = 0; i < A.GetLength(0); i++)
            {
                Max = MaxAbs(A, i);
                A = exchange(i, Max, A);
                double[] J = new double[A.GetLength(0) - i - 1];
                int k = 0;
                for (int j = i + 1; j < A.GetLength(0); j++)
                {
                    J[k] = A[j, i].num / A[i, i].num;
                    k++;
                }
                k = 0;
                for (int j = i + 1; j < A.GetLength(0); j++)
                {
                    for (int j1 = i + 1; j1 < A.GetLength(1); j1++)
                    {
                        A[j, j1].num -= (J[k] * A[i, j1].num);
                    }
                    k++;
                    A[j, i].num = 0;
                }
            }
            return A;
        }

        private double[] Eva(Mat[,] A)
        {
            A = Eli(A);
            double[] x = new double[A.GetLength(0)];
            x[A.GetLength(0) - 1] = A[A.GetLength(0) - 1, A.GetLength(1) - 1].num / A[A.GetLength(0) - 1, A.GetLength(1) - 2].num;
            for (int k = A.GetLength(0) - 2; k >= 0; k--)
            {
                double temp = 0;
                for (int i = k + 1; i < A.GetLength(0); i++)
                {
                    temp += (A[k, i].num * x[i]);
                }
                temp = A[k, A.GetLength(1) - 1].num - temp;
                x[k] = temp / A[k, k].num;
            }
            return x;
        }

        private double CalculateValue(double x, double y, double[] P)
        {
            int num = mInitPoint.Count;
            Mat[,] A = new Mat[num + 1, num + 2];
            double[] W = new double[num + 1];
            double temp_h = 0;
            double z = 0;
            for (int i = 0; i < (num + 1); i++)
            {
                for (int j = 0; j < (num + 2); j++)
                {
                    A[i, j].row = i;
                    A[i, j].col = j;
                    if (i == j)
                        continue;
                    else if ((j == num) || (i == num))
                        A[i, j].num = 1;
                    else if (j == (num + 1))
                    {
                        temp_h = Math.Sqrt(Math.Pow((mInitPoint[i].x - x), 2) + Math.Pow((mInitPoint[i].y - y), 2));
                        //A[i, j].num = P[0] + P[1] * (1 - Math.Exp(-Math.Pow((temp_h / P[2]), 2)));
                        A[i, j].num = P[0] + P[1] * (3.0 / 2.0 * temp_h / P[2] - Math.Pow(temp_h, 3) / (2 * Math.Pow(P[2], 3)));
                    }
                    else
                    {
                        temp_h = Math.Sqrt(Math.Pow((mInitPoint[i].x - mInitPoint[j].x), 2) + Math.Pow((mInitPoint[i].y - mInitPoint[j].y), 2));
                        //A[i, j].num = P[0] + P[1] * (1 - Math.Exp(-Math.Pow((temp_h / P[2]), 2)));
                        A[i, j].num = P[0] + P[1] * (3.0 / 2.0 * temp_h / P[2] - Math.Pow(temp_h, 3) / (2 * Math.Pow(P[2], 3)));
                    }
                }
            }
            W = Eva(A);
            for (int i = 0; i < num; i++)
                z += W[i] * mInitPoint[i].z;
            return z;
        }
    }
}
